﻿
// 2019/1/18: 首个记录
// 2019/3/22: 时间戳赋值
// 2019/3/28: 修正朝向插值问题
// 2019/3/29: 添加注释
// 2019/7/2: 增加GNSS Time插值
// 2019/8/27: 增加toGeneralSample和Title

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using ASEva;

namespace GnssImuSampleV5
{
    enum GnssImuLocationMode
    {
        NoLocation = 0, //无位置信息
        Normal = 1, //默认模式
        RTKFixed = 2, //RTK固定解
        RTKFloat = 3, //RTK浮动解
        RTD = 4, //RTD
    };

    class GnssImuSample : Sample
    {
        public GnssImuLocationMode LocationMode { get; set; } //位置模式

        public double? Longitude { get; set; } // 经度 deg 东经为正
        public double? Latitude { get; set; } // 纬度 deg 北纬为正
        public double? Altitude { get; set; } // 海拔 m
        public double? Speed { get; set; } // 车速 kph
        public double? Orientation { get; set; } // 朝向 deg CCW为正, 北为0, -180~180

        public double? Pitch { get; set; } // 俯仰角 deg 车头朝下为正
        public double? Roll { get; set; } // 横滚角 deg 右侧朝下为正
        public double? YawRate { get; set; } // 横摆角速度 deg/s CCW为正
        public double? Ax { get; set; } // 纵向加速度 m/s2 朝前为正
        public double? Ay { get; set; } // 横向加速度 m/s2 朝左为正
        public double? Az { get; set; } // 天向加速度 m/s2 朝上为正

        public DateTime? GnssTime { get; set; } //GPS时间

        public override bool FromGeneralSample(GeneralSample sample)
        {
            if (sample.Protocol == "gnssimu-sample-v4")
            {
                if (sample.Values.Count != 18) return false;

                Longitude = sample.Values[0].ToDouble();
                Latitude = sample.Values[1].ToDouble();
                Altitude = sample.Values[2].ToDouble();
                Speed = sample.Values[3].ToDouble();
                Orientation = sample.Values[4].ToDouble();
                Pitch = sample.Values[5].ToDouble();
                Roll = sample.Values[6].ToDouble();
                YawRate = sample.Values[7].ToDouble();
                Ax = sample.Values[8].ToDouble();
                Ay = sample.Values[9].ToDouble();
                Az = sample.Values[10].ToDouble();

                if (sample.Values[11].IsNumber() &&
                    sample.Values[12].IsNumber() &&
                    sample.Values[13].IsNumber() &&
                    sample.Values[14].IsNumber() &&
                    sample.Values[15].IsNumber() &&
                    sample.Values[16].IsNumber() &&
                    sample.Values[17].IsNumber())
                {
                    GnssTime = new DateTime(sample.Values[11].ToInt().Value,
                        sample.Values[12].ToInt().Value,
                        sample.Values[13].ToInt().Value,
                        sample.Values[14].ToInt().Value,
                        sample.Values[15].ToInt().Value,
                        sample.Values[16].ToInt().Value,
                        sample.Values[17].ToInt().Value);
                }
                else
                {
                    GnssTime = null;
                }

                Base = sample.Base;
                Offset = sample.Offset;
                Timeline = sample.Timeline;

                return true;
            }
            else if (sample.Protocol == "gnssimu-sample-v5")
            {
                if (sample.Values.Count != 19) return false;

                LocationMode = sample.Values[0].IsNumber() ? (GnssImuLocationMode)(int)sample.Values[0].number : GnssImuLocationMode.NoLocation;
                Longitude = sample.Values[1].ToDouble();
                Latitude = sample.Values[2].ToDouble();
                Altitude = sample.Values[3].ToDouble();
                Speed = sample.Values[4].ToDouble();
                Orientation = sample.Values[5].ToDouble();
                Pitch = sample.Values[6].ToDouble();
                Roll = sample.Values[7].ToDouble();
                YawRate = sample.Values[8].ToDouble();
                Ax = sample.Values[9].ToDouble();
                Ay = sample.Values[10].ToDouble();
                Az = sample.Values[11].ToDouble();

                if (sample.Values[12].IsNumber() &&
                    sample.Values[13].IsNumber() &&
                    sample.Values[14].IsNumber() &&
                    sample.Values[15].IsNumber() &&
                    sample.Values[16].IsNumber() &&
                    sample.Values[17].IsNumber() &&
                    sample.Values[18].IsNumber())
                {
                    GnssTime = new DateTime(sample.Values[12].ToInt().Value,
                        sample.Values[13].ToInt().Value,
                        sample.Values[14].ToInt().Value,
                        sample.Values[15].ToInt().Value,
                        sample.Values[16].ToInt().Value,
                        sample.Values[17].ToInt().Value,
                        sample.Values[18].ToInt().Value);
                }
                else
                {
                    GnssTime = null;
                }

                Base = sample.Base;
                Offset = sample.Offset;
                Timeline = sample.Timeline;

                return true;
            }

            return false;
        }
        public override GeneralSample ToGeneralSample()
        {
            var sample = new GeneralSample();
            sample.Base = Base;
            sample.Offset = Offset;
            sample.Timeline = Timeline;
            sample.Protocol = GetGeneralSampleProtocol();

            var data = new GeneralSampleValue[19];
            data[0] = new GeneralSampleValue((double)(int)LocationMode);
            if (Longitude != null) data[1] = new GeneralSampleValue(Longitude.Value);
            if (Latitude != null) data[2] = new GeneralSampleValue(Latitude.Value);
            if (Altitude != null) data[3] = new GeneralSampleValue(Altitude.Value);
            if (Speed != null) data[4] = new GeneralSampleValue(Speed.Value);
            if (Orientation != null) data[5] = new GeneralSampleValue(Orientation.Value);
            if (Pitch != null) data[6] = new GeneralSampleValue(Pitch.Value);
            if (Roll != null) data[7] = new GeneralSampleValue(Roll.Value);
            if (YawRate != null) data[8] = new GeneralSampleValue(YawRate.Value);
            if (Ax != null) data[9] = new GeneralSampleValue(Ax.Value);
            if (Ay != null) data[10] = new GeneralSampleValue(Ay.Value);
            if (Az != null) data[11] = new GeneralSampleValue(Az.Value);
            if (GnssTime != null)
            {
                data[12] = new GeneralSampleValue(GnssTime.Value.Year);
                data[13] = new GeneralSampleValue(GnssTime.Value.Month);
                data[14] = new GeneralSampleValue(GnssTime.Value.Day);
                data[15] = new GeneralSampleValue(GnssTime.Value.Hour);
                data[16] = new GeneralSampleValue(GnssTime.Value.Minute);
                data[17] = new GeneralSampleValue(GnssTime.Value.Second);
                data[18] = new GeneralSampleValue(GnssTime.Value.Millisecond);
            }

            sample.Values = data.ToList();
            sample.NumberOfSignificants = 19;

            return sample;
        }
        public override string GetGeneralSampleProtocol()
        {
            return "gnssimu-sample-v5";
        }
        public static string[] Protocols
        {
            get
            {
                return new String[] { "gnssimu-sample-v4", "gnssimu-sample-v5" };
            }
        }
        public static string Title
        {
            get
            {
                return "Location mode,Longitude[°],Latitude[°],Altitude[m],Speed[KPH],Orientation[°],Pitch angle[°],Roll angle[°],Yaw rate[°/s],Acceleration-x[m/s2],Acceleration-y[m/s2],Acceleration-z[m/s2],Year,Month,Day,Hour,Minute,Second,Millisecond";
            }
        }
        public override string[] GetGeneralSampleProtocols()
        {
            return Protocols;
        }
        protected override Sample Interpolate(Sample.SearchResult input)
        {
            var s1 = input.s1 as GnssImuSample;
            var s2 = input.s2 as GnssImuSample;
            var w1 = input.w1;
            var w2 = input.w2;

            var output = new GnssImuSample();

            if (s1.LocationMode == s2.LocationMode)
            {
                output.LocationMode = s1.LocationMode;
                output.Longitude = s1.Longitude * w1 + s2.Longitude * w2;
                output.Latitude = s1.Latitude * w1 + s2.Latitude * w2;
            }
            else
            {
                output.LocationMode = w1 > w2 ? s1.LocationMode : s2.LocationMode;
                output.Longitude = w1 > w2 ? s1.Longitude : s2.Longitude;
                output.Latitude = w1 > w2 ? s1.Latitude : s2.Latitude;
            }

            output.Altitude = s1.Altitude * w1 + s2.Altitude * w2;
            output.Speed = s1.Speed * w1 + s2.Speed * w2;
            output.Orientation = interpolateAngle(s1.Orientation, w1, s2.Orientation, w2);
            output.Pitch = s1.Pitch * w1 + s2.Pitch * w2;
            output.Roll = s1.Roll * w1 + s2.Roll * w2;
            output.YawRate = s1.YawRate * w1 + s2.YawRate * w2;
            output.Ax = s1.Ax * w1 + s2.Ax * w2;
            output.Ay = s1.Ay * w1 + s2.Ay * w2;
            output.Az = s1.Az * w1 + s2.Az * w2;

            if (s1.GnssTime != null && s2.GnssTime != null)
            {
                var diffSecs = (s2.GnssTime.Value - s1.GnssTime.Value).TotalSeconds;
                output.GnssTime = s1.GnssTime.Value.AddSeconds(diffSecs * w2 / (w1 + w2));
            }

            return output;
        }
        public override bool SupportInterpolation()
        {
            return true;
        }

        private double? interpolateAngle(double? a1, double w1, double? a2, double w2)
        {
            if (a1 == null || a2 == null) return null;

            var deg2rad = Math.PI / 180;
            var x1 = Math.Cos(a1.Value * deg2rad);
            var y1 = Math.Sin(a1.Value * deg2rad);
            var x2 = Math.Cos(a2.Value * deg2rad);
            var y2 = Math.Sin(a2.Value * deg2rad);

            var xo = x1 * w1 + x2 * w2;
            var yo = y1 * w1 + y2 * w2;
            if (xo == 0 && yo == 0) return null;

            return Math.Atan2(yo, xo) / deg2rad;
        }
    }
}
